//29 mar: upgraded version allowing memory of last write spot
//20mar:  separating flashing and dumping.
//        this code for Bob himself
//        he samples compass, writes to dnt, and writes to his nand.
//!!!!!careful, over-writes on each restart!!!!!!
//19mar cutting together with new lm303 choices
//
//15Mar2013 KAL merging of jacob's FlashLibraryTest and peter's BobDnt200
//

#include <Wire.h>
#include "LSM303.h"

// inslude the SPI library:
#include "Flash.h"
#include <SPI.h>
#include <Serial.h>
#include <EEPROM.h>

#define SAMPLE_INTERVAL 2500 // 400 Hz
#define MAG_SAMPLE_INTERVAL 14925 // 67 Hz

#define SENTENCE_LENGTH 6
#define MAG_SENTENCE_LENGTH 12

Flash* nandFlash;

// Serial communication between compass and Arduino
LSM303 compass;

// At the start of each line of data
byte k_sentinel = '$'; // When line doesn't have mag data
byte k_mag_sentinel= '#'; // When line has all data

void setup() 
{  
  Serial.begin(115200);

  nandFlash = new Flash();

   // Compass setup
  Wire.begin();
  compass.init(); //Turns on LSM303DLH
 
  // compass.writeAccReg(LSM303_CTRL_REG1_A, 0x3F); // Acc output data rate (ODR) = 1000 Hz
  // this is also low power
  compass.writeAccReg(LSM303_CTRL_REG1_A, 0x77); // Acc output data rate (ODR) = 400 Hz, not low power
 
 // compass.writeAccReg(LSM303_CTRL_REG4_A, 0x30); // Set accelerometer scale to +/-8 g
 // with new code, this is actually 16g....
  compass.writeAccReg(LSM303_CTRL_REG4_A, 0x38); // Set accelerometer scale to +/-16 g w HR
  compass.writeMagReg(LSM303_MR_REG_M, 0x00);    // Default
  compass.writeMagReg(LSM303_CRA_REG_M, 0x18);   // Mag output data rate (ODR) = 75 Hz (max)
  
  delay(1000);  
}

void loop()
{
  // This will have to be treated as an array of bytes later
  byte data[SENTENCE_LENGTH * 2];

  // Data time stamp (in milliseconds)
  unsigned long time = 0; // Time of last/current sample
  unsigned long prev_time = 0; // Time of last mag sample
  
  int byteCount = 0;
  
  //while (Serial.available() == 0);
  
  while (1)
  {
    if (byteCount > 2000)
    {
      nandFlash->cacheToArray();
      byteCount = 0;
    }
    
    if ((micros() - time) > SAMPLE_INTERVAL)
    {
      // Collect data as close to time stamp as possible
      time = micros();
      compass.readAcc();
      
      int ax = (int)compass.a.x;
      data[0] = ax & 0xFF;
      data[1] = (ax >> 8) & 0xFF;
      
      int ay = (int)compass.a.y;
      data[2] = ay & 0xFF;
      data[3] = (ay >> 8) & 0xFF;
      
      int az = (int)compass.a.z;
      data[4] = az & 0xFF;
      data[5] = (az >> 8) & 0xFF;
      
      byte time0 = time & 0xFF;
      byte time1 = (time >> 8) & 0xFF;
      byte time2 = (time >> 16) & 0xFF;
      byte time3 = (time >> 24) & 0xFF;
      
      if ((time - prev_time) > MAG_SAMPLE_INTERVAL)
      {
        compass.readMag();
        int mx = (int)compass.m.x;
        data[6] = mx & 0xFF;
        data[7] = (mx >> 8) & 0xFF;
        
        int my = (int)compass.m.y;
        data[8] = my & 0xFF;
        data[9] = (my >> 8) & 0xFF;
        
        int mz = (int)compass.m.z;
        data[10] = mz & 0xFF;
        data[11] = (mz >> 8) & 0xFF;
        
        // Send data over the radio
        // Serial.println("mag to dnt");
        Serial.write(k_mag_sentinel);
        
        Serial.write(time0);
        Serial.write(time1);
        Serial.write(time2);
        Serial.write(time3);
        
        for (int s = 0; s < 12; s++)
        {
          Serial.write(data[s]);
        }

        //send it to nandflash
        nandFlash->writeBytes(&k_mag_sentinel, 1);
        
        nandFlash->writeBytes(&time0, 1);
        nandFlash->writeBytes(&time1, 1);
        nandFlash->writeBytes(&time2, 1);
        nandFlash->writeBytes(&time3, 1);
        
        nandFlash->writeBytes(data, MAG_SENTENCE_LENGTH);

        prev_time = time;
        
        byteCount += 17; // 1 + sizeof(long) + MAG_SENTENCE_LENGTH * sizeof(int);
      }
      else
      {
        // Send data over the radio
        //Serial.println("acc to dnt");
        Serial.write(k_sentinel);
        
        Serial.write(time0);
        Serial.write(time1);
        Serial.write(time2);
        Serial.write(time3);
        
        for (int s = 0; s < 6; s++)
        {
          Serial.write(data[s]);
        }
        
        //send it to nandflash
        nandFlash->writeBytes(&k_sentinel, 1);
        
        nandFlash->writeBytes(&time0, 1);
        nandFlash->writeBytes(&time1, 1);
        nandFlash->writeBytes(&time2, 1);
        nandFlash->writeBytes(&time3, 1);

        nandFlash->writeBytes(data, SENTENCE_LENGTH);

        byteCount += 11; // 1 + sizeof(long) + SENTENCE_LENGTH * sizeof(int);
      }
    }
  }
}
